#include "Container.h"

Container::Container() 
{
	gap = 4.0;
	//size = 0;
	currentLabel = 0;
	lowThreshold = 0.90; // 0.90 is tested with crossing road and CAVIAR
}

Container::~Container() {}

// Handle the first frame
int Container::InsertNewTracklet(CvRect rect, int iframe)
{
	Tracklet *newTracklet = new Tracklet();
	newTracklet->trackletSet.push_back(rect);	
	this->tracklets.push_back(*newTracklet);
	tracklets.back().label = currentLabel++;
	tracklets.back().frame.push_back(iframe);
	//size++;
	/*char strLabel[4];
	itoa(size, strLabel, 10);
	tracklets[size-1].label.append(strLabel);
	return tracklets[size-1].label;*/
	return (currentLabel-1);
}

int Container::FindApproTracklet(CvRect rect, int iframe)
{
	lastFrame = iframe;
	int i;
	int pos = -1;
	CvRect rect0;
	double d;
	//vector<int>::size_type sz = tracklets.size();
	//unsigned int pos = -1; //tracklet number that a track response is belong to 

	double plink_max = 0;
	int iTracklet = -1;	// the tracklet that this response may join in
	for (i = 0; i < tracklets.size(); i++)
	{
		if (iframe - (int)tracklets[i].frame.back() != 1)
		{
			continue;
		}

		rect0 = ((CvRect)tracklets[i].trackletSet.back());

		double Apos = 0.00001;	// 0 if perfectly match
		int x1 = rect.x + rect.width/2;
		int y1 = rect.y + rect.height/2;
		int x2 = rect0.x + rect0.width/2;
		int y2 = rect0.y + rect0.height/2;
		if (x1 != x2 && y1 != y2)
		{
			//Apos = exp(-(rect.x-rect0.x)*(rect.x-rect0.x)/Utils::ComputeVariance(rect.x,rect0.x))
			//	* exp(-(rect.y-rect0.y)*(rect.y-rect0.y)/Utils::ComputeVariance(rect.y,rect0.y));
			Apos = exp(-abs(x1-x2)/Utils::ComputeVariance(x1,x2))
					* exp(-abs(y1-y2)/Utils::ComputeVariance(y1,y2));
		}
		
		int size1 = rect.width*rect.height;
		int size2 = rect0.width*rect0.height;
		double Asize = 0.5;	// 0 if perfectly match
		if (size1 != size2)
		{
			//Asize = exp(-(size1-size2)*(size1-size2)/Utils::ComputeVariance(size1, size2));
			Asize = exp(-10*abs(size1-size2)/Utils::ComputeVariance(size1, size2));
		}

		// load previous frame
		char str[3] = "1";
		string fileName = "image1\\x01 (";
		itoa(iframe-1,str,10);
		fileName.append(str);
		fileName.append(").jpg");
		IplImage *img1 = cvLoadImage((char *)fileName.c_str(), 1);

		// this frame
		fileName = "image1\\x01 (";
		itoa(iframe,str,10);
		fileName.append(str);
		fileName.append(").jpg");
		IplImage *img2 = cvLoadImage((char *)fileName.c_str(), 1);

		// Bhattachayya Distance
		Hist* h1 = new Hist(cvGetSize(img1));
		Hist* h2 = new Hist(cvGetSize(img2));
		double Aapp = h1->computeBhattachayyaDistance(h1->computeCvHist(img1,rect0),h2->computeCvHist(img2,rect));

		double plink = 1-Apos*Asize*Aapp;

		//cvSetImageROI(img1,rect0);
		//cvSetImageROI(img2,rect);
		//cvShowImage("0",img1);
		//cvShowImage("2",img2);
		//cout << "x1=" << x1 << " x2=" << x2 << " y1=" << y1 << " y2=" << y2
		//	<< " s1=" << size1 << " s2=" << size2 << endl;
		//cout << Apos << "\t" << Asize << "\t" << Aapp << "\tPlink=" << plink << endl;
		//cvWaitKey();
		delete h1;
		delete h2;
		//cvReleaseImage(&img1);
		//cvReleaseImage(&img2);

		if (plink > lowThreshold)
		{
			if (plink > plink_max)
			{
				plink_max = plink;
				iTracklet = i;
			}
		}

		//d = CalDistance2Object(rect0, rect);
		//if (d <= gap) // detector is belong to the tracklet i
		//{
		//	tracklets[i].trackletSet.push_back(rect);
		//	tracklets[i].frame.push_back(iframe);
		//	tracklets[i].active.push_back(1);
		//	//tracklets[i].label = i;
		//	pos = i;
		//}
	}

	if (iTracklet != -1)
	{
		tracklets[iTracklet].trackletSet.push_back(rect);
		tracklets[iTracklet].frame.push_back(iframe);
		//tracklets[i].label = i;
		pos = iTracklet;
		cout << "Joint to tracklet: " << pos << "\n";
	}	

	// new tracklet
	if (pos == -1)
	{
		Tracklet *newTracklet = new Tracklet();
		newTracklet->trackletSet.push_back(rect);

		tracklets.push_back(*newTracklet);
		tracklets.back().label = currentLabel++;
		tracklets.back().frame.push_back(iframe);
		pos = tracklets.size() - 1;
		//size++;
		cout << "New tracklet: " << pos << "\n";

		// update discriminative set
		for (i = 0; i < tracklets.size(); i++)
		{
			if (i != pos)
			{
				tracklets[i].discrmSet.push_back(pos);
				tracklets[pos].discrmSet.push_back(i);
			}		
		}
	}
	
	return pos;
}

void Container::RemoveDuplicateDSet()
{
	vector<int>::iterator new_end_pos;
	for (int i = 0; i < tracklets.size(); i++)
	{
		std::sort( tracklets[i].discrmSet.begin(), tracklets[i].discrmSet.end() );		
		new_end_pos = std::unique( tracklets[i].discrmSet.begin(), tracklets[i].discrmSet.end() );
		tracklets[i].discrmSet.erase( new_end_pos, tracklets[i].discrmSet.end() );
	}
}

double Container::CalDistance2Object(CvRect rect1, CvRect rect2)
{
	CvPoint p1 = cvPoint(rect1.x + rect1.width/2, rect1.y + rect1.height/2);
	CvPoint p2 = cvPoint(rect2.x + rect2.width/2, rect2.y + rect2.height/2);
	double d;
	d = sqrt((p1.x - p2.x)*(p1.x - p2.x)*1.0 + (p1.y - p2.y)*(p1.y - p2.y));

	return d;
}

int Container::GetTrackletLabel(int index)
{
	return tracklets[index].label;
}

void Container::PrepareNextProcess(int ignoreFrame)
{
	for (int i = 0; i < tracklets.size(); i++)
	{
		if (tracklets[i].frame[0] == ignoreFrame)
		{
			tracklets[i].frame.erase(tracklets[i].frame.begin());
			tracklets[i].trackletSet.erase(tracklets[i].trackletSet.begin());			
		}		
		if (tracklets[i].frame.size() == 0) // in case this is the only element in the tracklet
		{
			tracklets.erase(tracklets.begin() + i);	// delete the empty tracklet
			// update tracklet's label as well as discriminative set
			for (int j = 0; j < tracklets.size(); j++)
			{
				//tracklets[j].label = j;
				tracklets[j].discrmSet.clear();
				for (int k = 0; k < tracklets.size(); k++)
				{
					if (k != j)
					{
						tracklets[j].discrmSet.push_back(k);
					}					
				}
			}
			//int key[] = {i};
			//vector<int>::iterator it;			
			//for (int j = 0; j < tracklets.size(); j++)
			//{
			//	it = search(tracklets[j].discrmSet.begin(),tracklets[j].discrmSet.end(),key,key+1);
			//	if (it != tracklets[j].discrmSet.end())
			//	{
			//		tracklets[j].discrmSet.erase(it);
			//	}
			//}
		}
	}
	//int i = 0;
	//do 
	//{
	//	if (!tracklets[i].NextProcess(this->lastFrame))
	//		//if (!tracklets[i].NextProcess())
	//	{
	//		tracklets.erase(tracklets.begin() + i);
	//	}
	//	else
	//	{
	//		i++;
	//	}
	//} while (i < tracklets.size());
	//size = tracklets.size();
}